#ifndef DOG_MOVE_H
#define DOG_MOVE_H

#include <delay.h>
#include <app.h>

#include "servo.h"
#include "lite_dog.h"

//定长设置
void Dog_Gravity_Data_Set_XY_Data(float Set_Gravity_Target,float* Legs_Gravity_Data);
//定高设置
void Dog_Height_Data_Set_XY_Data(float Set_Height_Target,float* Legs_Height_Data);

//姿态设置
void Lite_Dog_Set_Attitude_Angle(Lite_Dog_Struct* Dog_Set_Attitude_Angle,float Pitch_Target,float Roll_Target,float Yaw_Target);

void Lite_Dog_Set_Attitude_Angle_WithoutMPU(Lite_Dog_Struct* Dog_Set_Attitude_Angle,float Pitch_Target,float Roll_Target,float Yaw_Target);

void Lite_Dog_Set_Attitude_XYZ(Lite_Dog_Struct* Dog_Set_Attitude_XYZ,float X_Set,float Y_Set,float Z_Set);

#define Body_Move_Lenth 

//步态设计
//#define Move_Lenth 20.00f

#define Move_Cal_Y(Set_Alpha,Now_Hei,Now_Len,Star_x,Star_y,Move_str)

#define Move_Cal_X(Set_Alpha,Now_Hei,Now_Len,Star_x,Star_y,Move_str)

#define Move_Oval_Cal(Set_Alpha,Now_Hei,Now_Len,Star_x,Star_y,Move_str)  \
Star_y-sqrt(Now_Hei*Now_Hei - Now_Hei*Now_Hei*(Set_Alpha-Move_str*Now_Len-Star_x)*(Set_Alpha-Move_str*Now_Len-Star_x)/(Now_Len*Now_Len))

//Trot_Cycloid步态-摆线
void Dog_Trot_Cycloid(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Trot_Move_Data,float* Leg_X,float* Leg_Y);
//Walk-Cycloid步态-摆线
void Dog_Walk_Cycloid(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Walk_Move_Data,float* Leg_X,float* Leg_Y);


//Trot_Oval步态 - 椭圆
void Dog_Trot_Oval(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Trot_Move_Data,float* Leg_X,float* Leg_Y);
void Dog_Trot_Step(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Trot_Move_Data,float* Leg_X,float* Leg_Y);

//Walk步态
void Dog_Walk_Oval(int Time,Dog_Set_Move_Data_Struct* Walk_Move_Data,float* Leg_X,float* Leg_Y);

//缓步移动
void Dog_Legs_Move_Step(Lite_Dog_Struct* Dog_Move_Step,int Move_Step_Set,int Target_X,int Target_Y);

void Dog_Motion_Data_Set(Dog_Set_Move_Data_Struct* Move_Data_Set,Dog_Sport_Mode_Enum Sport_Mode_Set,Dog_Move_Mode_Enum Move_Mode_Get,float Move_Height,float Move_Lenth,float Move_Speed);

void Dog_Motion_XY_Trot_Set(Dog_Set_Move_Data_Struct* Move_XY_Set,float* Trot_X_Cood,float* Trot_Y_Cood);

void Dog_Motion_XY_Walk_Set(Dog_Set_Move_Data_Struct* Move_XY_Set,float* Trot_X_Cood,float* Trot_Y_Cood);

void Dog_Motion_XY_Set(Dog_Set_Move_Data_Struct* Move_XY_Set,float* Move_X_Cood,float* Move_Y_Cood);


#endif
